// World.cpp

// Implementacia modelu sveta

#include "Player/World/World.h"
#include "Player/PlayerKernel.h"
//#include "Common/Main.h"
#include "Common/Tasks.h"
#include "Common/Support/Debug.h"
#include <string.h>

#include "Evaluating/EvaluatingPanesMap.h"
#include "Evaluating/EvaluatingPlayerList.h"

// Ohodnocovacia mapa
extern CEvaluatingPanesMap* ePanesMap;

// Zoznam informacii o hracoch, ktorych vidi hrac
extern CEvaluatingPlayerList* ePlayerList;

extern bool EvaluatingPanes;

SynchronizationInterface* World::synchro = NULL;

// Konstruktor
World::World()
{
	memset((void*) friendTypes, 0, 12 * sizeof(PLAYER_TYPE*));

	ball		= new Ball(this);
	state		= new State(this);
	variables	= new Variables(this);
	params		= new Params(this);
	me			= new Myself(this);
}

// Kopirovaci konstruktor
World::World(const World& world)
{
	ball = new Ball(*world.ball);
	ball->SetWorld(this);
	
	me = new Myself(*world.me);
	me->SetWorld(this);
	
	state = new State(*world.state);
	state->SetWorld(this);
	
	params = new Params(*world.params);
	params->SetWorld(this);
	
	variables = new Variables(*world.variables);
	variables->SetWorld(this);

	int i;
	
	for(i = 0; i < world.friends.GetSize(); i++)
	{
		friends.Add(world.friends[i]);
		friends[i].SetWorld(this);
	}
	for(i = 0; i < world.enemies.GetSize(); i++)
	{
		enemies.Add(world.enemies[i]);
		enemies[i].SetWorld(this);
	}

	memcpy((void*) friendTypes, (void*) world.friendTypes, 12 * sizeof(PLAYER_TYPE*));
}

World::~World()
{
	if(ball			!= NULL)	delete ball;
	if(me			!= NULL)	delete me;
	if(state		!= NULL)	delete state;
	if(params		!= NULL)	delete params;
	if(variables	!= NULL)	delete variables;
}


Player* World::FindEnemy(int uniform) const
{
	int index = FindEnemyIndex(uniform);
	
	if(index == -1)
		return NULL;
	else 
		return &enemies[index];
}

int World::FindEnemyIndex(int uniform) const
{
	if(uniform == 0)
		return -1;

	for(int i = 0; i < enemies.GetSize(); i++)
	{
		if(enemies[i].GetUniform() == uniform)
			return i;
	}

	return -1;
}

Player* World::FindFriend(int uniform) const
{
	int index = FindFriendIndex(uniform);
	
	if(index == -1)
		return NULL;
	else 
		return &friends[index];
}

int World::FindFriendIndex(int uniform) const
{
	if(uniform == 0)
		return -1;

	for(int i = 0; i < friends.GetSize(); i++)
	{
		if(friends[i].GetUniform() == uniform)
			return i;
	}

	return -1;
}

void World::ApplyFeedback(FEEDBACK_DATA* fd)
{	
	// Vyuziju sa sucasne prikazy ulozene v strukture FEEDBACK_DATA na modifikovanie sveta
	
	// Aplikovanie sucasneho prikazu "dash"
	if(fd->lastPower != 0)
	{
		// Vypocita sa skutocna "dash" energia
		float energy = fd->lastPower * me->type->dash_power_rate * me->GetEffort();
		if(energy > params->server->player_accel_max)
		{
			energy = params->server->player_accel_max;
		}
		else if(energy < -params->server->player_accel_max)
		{
			energy = -params->server->player_accel_max;
		}

		me->vel += PolarVector(energy, me->GetAbsDir());

		if(me->vel.GetLength() > me->type->player_speed_max)
		{
			me->vel *= (me->type->player_speed_max / me->vel.GetLength());
		}

		fd->lastPower = 0;
	}

	// Aplikovanie sucasneho prikazu "turn"	
	if(fd->lastTurn != 0)
	{
		Angle angle = me->GetAbsDir() + DegAngle(fd->lastTurn / (1 + me->vel.GetLength() * me->type->inertia_moment));
		me->SetAbsDir(angle);

		fd->lastTurn = 0;
	}

	// Aplikovanie sucasneho prikazu "turn neck"
	if(fd->lastNeck != 0)
	{
		me->SetNeckDir(me->GetNeckDir() + DegAngle((float) fd->lastNeck));
		if(me->GetNeckDir().GetDeg() > params->server->maxneckang)
		{
			me->SetNeckDir(DegAngle(params->server->maxneckang));
		}
		else if(me->GetNeckDir().GetDeg() < params->server->minneckang)
		{
			me->SetNeckDir(DegAngle(params->server->minneckang));
		}

		fd->lastNeck = 0;
	}

	// Aplikovanie sucasneho prikazu "kick"
	if(fd->lastKickPower != 0)
	{
		Vector diff = ball->pos - me->pos;
		float ballDist = diff.GetLength();

		// Ak je lopta dostatocne blizko
		if(ballDist < (me->type->player_size + me->type->kickable_margin + params->server->ball_size))
		{
			if(ballDist < (me->type->player_size + params->server->ball_size))
			{
				ballDist = me->type->player_size + params->server->ball_size;
			}

			float dirDiff = (diff.GetAngle() - me->GetAbsDir()).GetAbsDeg();
			
			float accel = fd->lastKickPower * params->server->kick_power_rate * (1 - 0.25f * (dirDiff / 180) 
				- 0.25f * (ballDist - me->type->player_size - params->server->ball_size) / me->type->kickable_margin);

			if(accel > params->server->ball_accel_max)
			{
				accel = params->server->ball_accel_max;
			}
			else if(accel < -params->server->ball_accel_max)
			{
				accel = -params->server->ball_accel_max;
			}

			Angle kickDir = me->GetAbsDir() + DegAngle(fd->lastKickDir);
			
			ball->vel += PolarVector(accel, kickDir);

			if(ball->vel.GetLength() > params->server->ball_speed_max)
			{
				ball->vel *= params->server->ball_speed_max / ball->vel.GetLength();
			}
		}

		fd->lastKickPower = 0;
	}
}


void World::BindInterfaces()
{
	synchro = (SynchronizationInterface*) Main::GetInstance()->GetInterface(SYNCHRONIZATION_TASK);
}


void World::ProcessInfo(const SENSE_BODY* sb, const VISUAL_INFO* vi)
{
	WORLD_RECALC_DBG("---> enter ProcessInfo at %i", state->GetSimTime());

	if(me->Recalculate(sb, vi))
	{
		WORLD_RECALC_DBG("  me: recalc ok, pos: (%3.2f, %3.2f), vel (%3.2f, %3.2f)", me->pos.GetX(), me->pos.GetY(), me->vel.GetX(), me->vel.GetY());
		WORLD_RECALC_DBG("  me: viewDir %3.2f absDir %3.2f neckDir %3.2f", (me->GetAbsDir() + me->GetNeckDir()).GetDeg(), me->GetAbsDir().GetDeg(), me->GetNeckDir().GetDeg());

		// Poznam svoju poziciu. Ak mozeme, tak prekalkulujme loptu
		if(vi->ballCnt != 0 && vi->ball.distValid)
		{
			WORLD_RECALC_DBG("  ball: I see ball");
			me->bNeckSynchronization = true;
	
			// Vidim loptu
			if(ball == NULL)
				ball = new Ball(this);
			ball->Recalculate(sb, vi);

			// Vidime loptu, zmen synchronizacny mod z WaitForVI na Normal
			if(synchro->GetNextSynchMode() == Synch_WaitForVI)
				synchro->SetNextSynchMode(Synch_Normal);
		}
		else
		{
			WORLD_RECALC_DBG("  ball: I do NOT see ball");
			me->bNeckSynchronization = false;

			// Nevidim loptu, simulujme to
			if(ball->IsValid() && me->IsLookingAt(ball->pos))
			{
				// Mal by som vidiet loptu, ale nevidim
				ball->SetValid(false);
				ball->vel = RectVector(0, 0);

				WORLD_RECALC_DBG("  ball: simulate: I should SEE ball");
			}

			if(ball->IsValid())
			{
				// Mal by som citit loptu (mala by byt blizko mna), ale necitim
				Vector diff = ball->pos - me->pos;
				if(diff.GetLength() <= params->server->visible_distance)
				{
					ball->SetValid(false);
					ball->vel = RectVector(0, 0);

					WORLD_RECALC_DBG("  ball: simulate: I should FEEL ball");
				}
			}

			WORLD_RECALC_DBG("  ball: SIMULATE: pos: (%f, %f), vel (%f, %f)", ball->pos.GetX(), ball->pos.GetY(), ball->vel.GetX(), ball->vel.GetY());
		}
		
		// Prekalkuluje hracov
		RecalculateFriends(sb, vi);
		RecalculateEnemies(sb, vi);
		
		// Uloz cas a poziciu mojej poslednej VI
		me->SaveLastSeenData();
	}
	else
	{
		WORLD_RECALC_DBG("  me: recalc BAD, pos: (%f, %f), vel (%f, %f)", me->pos.GetX(), me->pos.GetY(), me->vel.GetX(), me->vel.GetY());

		// Nepoznam svoju poziciu
		ball->SetValid(false);
		ball->vel = RectVector(0, 0);

		WORLD_RECALC_DBG("  ball: setting to invalid: pos: (%f, %f), vel (%f, %f)", ball->pos.GetX(), ball->pos.GetY(), ball->vel.GetX(), ball->vel.GetY());
	}

	WORLD_RECALC_DBG("<--- exit ProcessInfo");
}



void World::RecalculateEnemies(const SENSE_BODY *sb, const VISUAL_INFO *vi)
{
	int i;

	// Pozeram sa tymto smerom
	Angle viewDir = me->GetAbsDir() + me->GetNeckDir();

	// Odstrania sa stari superi a superi, ktorych by som mal vidiet
	for(i = enemies.GetSize() - 1; i >= 0; i--)
	{
		if(enemies[i].GetAge() >= MAX_PLAYER_AGE || me->IsLookingAt(enemies[i].pos))
			enemies.RemoveAt(i);
	}
	
	// Pridaju sa superi, ktorych vidim
	for(i = 0; i < vi->theirCnt; i++)
	{
		if(!vi->their[i].valid)
			DBGN("vi->their[i].valid is false");

		Player player(this);

		// Vypocitaj absolutnu poziciu hraca
		Angle angle = viewDir + DegAngle(vi->their[i].dir);
		player.pos = me->pos + PolarVector(vi->their[i].dist, angle);

		// Vypocita sa rychlost hraca
		if(vi->their[i].changesValid) // vidime rychlost
		{
			// Vidim jeho rychlost
			// (Predchodcovia to prebrali od TsinghuAeolus2002)
			Vector rpos = PolarVector(vi->their[i].dist, DegAngle(vi->their[i].dir));
			rpos /= rpos.GetLength();

			Vector rvel = RectVector(
				vi->their[i].distChange * rpos.GetX() - vi->their[i].dirChange * M_PI / 180.f * vi->their[i].dist * rpos.GetY(),
				vi->their[i].distChange * rpos.GetY() + vi->their[i].dirChange * M_PI / 180.f * vi->their[i].dist * rpos.GetX());
				
			// Rotuj rvel
			rvel += viewDir;
			player.vel = me->vel + rvel;
			
//			float dirCh = vi->their[i].dirChange * M_PI / 180.f;
//			player.vel = me->vel - PolarVector(vi->their[i].distChange, angle) - 
//				PolarVector(vi->their[i].dist * dirCh, angle).OrthoLeft();
		}
		else
		{	
			// Neznama rychlost, nastavi sa na 0
			player.vel = RectVector(0,0);
		}

		// Smer hlavy a tela
		if(vi->their[i].facingValid)
		{
			player.SetAbsDir(viewDir + DegAngle(vi->their[i].bodyFacingDir));
			player.SetNeckDir(viewDir + DegAngle(vi->their[i].headFacingDir));
			player.SetDirValid();
		}

		// Smer ramena
		if(vi->their[i].armValid)
		{
			player.SetArmDir(viewDir + DegAngle(vi->their[i].armDir));
			player.SetArmValid();
		}

		player.SetSide(THEIR);
		player.SetUniform(vi->their[i].uniformNumber);
		player.SetGoalie(vi->their[i].goalie);
		player.SetAge(0);
		
		if(EvaluatingPanes)
		{
			// Nove informacie o nepriatelskych hracoch do ohodnocovacej mapy
			CEvaluatingPlayer* pEvEnemyPlayer	= new CEvaluatingPlayer(PT_THEIR, vi->their[i].uniformNumber, player.pos.GetX(), player.pos.GetY(), ((vi->their[i].uniformNumber != 1) ? nInfluencePower : (int) (nInfluencePower * 1.5)), ((vi->their[i].uniformNumber != 1) ? nInfluenceSize : (int) (nInfluenceSize * 1.5)), player.GetAbsDir().GetDeg(), player.vel);
			ePlayerList->InsertPlayerInformation(pEvEnemyPlayer);
		}

		// Ak mame uz takeho hraca, nahrad ho, inak pridaj noveho
		int index = FindEnemyIndex(player.GetUniform());
		if(index == -1) 
			enemies.Add(player);
		else 
			enemies[index] = player;
	}
	
	// Pridaj neznamych hracov blizko mna, povazuju sa za nepriatelov
	for(i = 0; i < vi->nearbyCnt; i++)
	{
		Player player(this);

		// Vypocitaj absolutnu poziciu hraca
		Angle angle = viewDir + DegAngle(vi->nearby[i].dir);
		player.pos = me->pos + PolarVector(vi->nearby[i].dist, angle);

		// Nepozname jeho rychlost - nastav ju na 0
		player.vel = RectVector(0,0);

		player.SetSide(UNKNOWN);
		player.SetUniform(0);
		player.SetGoalie(false);
		
		if(EvaluatingPanes)
		{
			// Nove informacie o blizkych hracoch do ohodnocovacej mapy
			CEvaluatingPlayer* pEvUnknownPlayer	= new CEvaluatingPlayer(PT_NEARBY, vi->nearby[i].uniformNumber, player.pos.GetX(), player.pos.GetY(), nInfluencePower, nInfluenceSize, 0, player.vel);
			ePlayerList->InsertPlayerInformation(pEvUnknownPlayer);
		}

		// Nechame tychto hracov v pamati iba do nasledujucej VI
		player.SetAge(MAX_PLAYER_AGE);

		enemies.Add(player);
	}

	// Pridaj neznamych hracov
	for(i = 0; i < vi->unknownCnt; i++)
	{
		Player player(this);

		// Vypocitaj absolutnu poziciu hraca
		Angle angle = viewDir + DegAngle(vi->unknown[i].dir);
		player.pos = me->pos + PolarVector(vi->unknown[i].dist, angle);

		// Nepozname jeho rychlost - nastav ju na 0
		player.vel = RectVector(0,0);

		if(EvaluatingPanes)
		{
			// Nove informacie o neznamych hracoch do ohodnocovacej mapy
			CEvaluatingPlayer* pEvUnknownPlayer	= new CEvaluatingPlayer(PT_UNKNOWN, vi->unknown[i].uniformNumber, player.pos.GetX(), player.pos.GetY(), nInfluencePower, nInfluenceSize, 0, player.vel);
			ePlayerList->InsertPlayerInformation(pEvUnknownPlayer);
		}


		player.SetSide(UNKNOWN);
		player.SetUniform(0);
		player.SetGoalie(false);
		
		// Nechame tychto hracov v pamati iba do nasledujucej VI
		player.SetAge(MAX_PLAYER_AGE);

		enemies.Add(player);
	}
}

	
void World::RecalculateFriends(const SENSE_BODY* sb, const VISUAL_INFO* vi)
{
	int i;

	// Pozeram tymto smerom
	Angle viewDir = me->GetAbsDir() + me->GetNeckDir();

	// Odstran starych spoluhracov a tych spoluhracov, ktorych by som mal vidiet
	for(i = friends.GetSize() - 1; i >= 0; i--)
	{
		if(friends[i].GetAge() >= MAX_PLAYER_AGE || me->IsLookingAt(friends[i].pos))
			friends.RemoveAt(i);
	}
	
	// Pridaj spoluhracov, ktorych vidim
	for(i = 0; i < vi->oursCnt; i++)
	{
		if(!vi->ours[i].valid)
			DBGN("vi->ours[i].valid is false");

		Player player(this);

		// Vypocita sa absolutna pozicia hraca
		Angle angle = viewDir + DegAngle(vi->ours[i].dir);
		player.pos = me->pos + PolarVector(vi->ours[i].dist, angle);

		// Vypocita sa rychlost hraca
		if(vi->ours[i].changesValid)
		{
			// Vidim jeho rychlost
			Vector rpos = PolarVector(vi->ours[i].dist, DegAngle(vi->ours[i].dir));
			rpos /= rpos.GetLength();

			Vector rvel = RectVector(
				vi->ours[i].distChange * rpos.GetX() - vi->ours[i].dirChange * M_PI / 180.f * vi->ours[i].dist * rpos.GetY(),
				vi->ours[i].distChange * rpos.GetY() + vi->ours[i].dirChange * M_PI / 180.f * vi->ours[i].dist * rpos.GetX());
				
			// Rotuj rvel
			rvel += viewDir;
			player.vel = me->vel + rvel;

//			float dirCh = vi->ours[i].dirChange * M_PI / 180.f;
//			player.vel = me->vel - PolarVector(vi->ours[i].distChange, angle) - 
//				PolarVector(vi->ours[i].dist * dirCh, angle).OrthoLeft();
		}
		else
		{
			// Neznama rychlost, nastavi sa na 0
			player.vel = RectVector(0, 0);
		}

		// Smerovanie hlavy a tela
		if(vi->ours[i].facingValid)
		{
			player.SetAbsDir(viewDir + DegAngle(vi->ours[i].bodyFacingDir));
			player.SetNeckDir(viewDir + DegAngle(vi->ours[i].headFacingDir));
			player.SetDirValid();
		}
		
		// Smer ramena
		if(vi->ours[i].armValid)
		{
			player.SetArmDir(viewDir + DegAngle(vi->ours[i].armDir));
			player.SetArmValid();
		}

		player.SetSide(OUR);
		player.SetUniform(vi->ours[i].uniformNumber);
		player.SetGoalie(vi->ours[i].goalie);
		player.SetAge(0);

		if(EvaluatingPanes)
		{
			// Nove informacie o nasich hracoch do ohodnocovacej mapy
			CEvaluatingPlayer* pEvOurPlayer	= new CEvaluatingPlayer(PT_OUR, vi->ours[i].uniformNumber, player.pos.GetX(), player.pos.GetY(), nInfluencePower, nInfluenceSize, player.GetAbsDir().GetDeg(), player.vel);
			ePlayerList->InsertPlayerInformation(pEvOurPlayer);
		}

		PLAYER_TYPE* type = friendTypes[player.GetUniform()];
		if(type != NULL) 
			player.SetType(type->id);

		// Ak mame uz takeho hraca, nahrad ho, inak pridaj noveho
		int index = FindFriendIndex(player.GetUniform());
		
		if(index == -1) 
			friends.Add(player);
		else 
			friends[index] = player;
	}
}


void World::RelinkPlayerTypes()
{
	int i;

	// Tato metoda sa vola, ked pride zo servera informacia o type hraca.
	// Resetujeme vsetky 'type' pointre vsetkych spoluhracov, aby ukazovali na default typ hraca.
	// Nastavi typy vsetkych hracov default na 0
	
	if(params->types[0] != NULL)
		for(i = 0; i < 12; i++)
			friendTypes[i] = params->types[0];
	
	for(i = 0; i < friends.GetSize(); i++)
		friends[i].SetType(0);
	for(i = 0; i < enemies.GetSize(); i++)
		enemies[i].SetType(0);

	me->SetType(0);

	// Nastav moju sucasnu staminu a usilie tiez na maximum
	me->ResetStamina();
	me->ResetEffort();
}


void World::Step()
{
	int i;

	// Seba
	me->Step();
	
	// Loptu
	if(ball != NULL)
		ball->Step();

#ifdef DEBUG
	int dFa, dEa, dFd, dEd;
	int dFna, dEna, dFnd, dEnd;
	dFa = dEa = dFd = dEd = 0;
	dFna = dEna = dFnd = dEnd = 0;
#endif // DEBUG

	// Spoluhracov
	for(i = 0; i < friends.GetSize(); i++)
	{
#ifdef DEBUG
		if(friends[i].IsArmValid()) 
			dFa++; 
		else 
			dFna++;
		
		if(friends[i].IsDirValid()) 
			dFd++; 
		else 
			dFnd++;
#endif // DEBUG

		friends[i].Step();
	}

	// Superov
	for(i = 0; i < enemies.GetSize(); i++)
	{
#ifdef DEBUG
		if(enemies[i].IsArmValid())
			dEa++; 
		else 
			dEna++;
		if(enemies[i].IsDirValid())
			dEd++; 
		else 
			dEnd++;
#endif // DEBUG

		enemies[i].Step();
	}

#ifdef DEBUG
	WORLD_STATS_DBG("--> Step at %i", state->GetSimTime());
	if(friends.GetSize() > 10) 
		WORLD_STATS_DBG("too many friends: %i", friends.GetSize());
	if(enemies.GetSize() > 11) 
		WORLD_STATS_DBG("too many enemies: %i", enemies.GetSize());

	WORLD_STATS_DBG("dFa: %i, dFna: %i", dFa, dFna);
	WORLD_STATS_DBG("dEa: %i, dEna: %i", dEa, dEna);
	WORLD_STATS_DBG("dFd: %i, dFnd: %i", dFd, dFnd);
	WORLD_STATS_DBG("dEd: %i, dEnd: %i", dEd, dEnd);
	WORLD_STATS_DBG("<-- Step");
#endif // DEBUG
}

void World::SetPlayerType(Side side, int uniform, int type)
{
	if (uniform >= 1 && uniform < 13)
	{
		if(side == OUR)
		{
			friendTypes[uniform] = params->types[type];	
		}
		else if (side == THEIR)
		{
			//enemieTypes[uniform] = params->types[type];	
		}
	}
}
